#include "tcp_server.h"
#include <iostream>

TCPServer::TCPServer( const std::string& ip_address, int port)
    : ip_(ip_address), port_(port), server_fd_(-1){
}

TCPServer::~TCPServer() {
    if (server_fd_ != -1) {
        close(server_fd_);
    }
}

bool TCPServer::start() {
    server_fd_ = socket(AF_INET, SOCK_STREAM, 0);
    if (server_fd_ == -1) {
        std::cerr << "Socket creation failed" << std::endl;
        return false;
    }

    sockaddr_in address;
    address.sin_family = AF_INET;
    address.sin_port = htons(port_);
    if (inet_pton(AF_INET, ip_.c_str(), &address.sin_addr) <= 0) {
        std::cerr << "Invalid address or address not supported" << std::endl;
        return false;
    }

    if (bind(server_fd_, (struct sockaddr*)&address, sizeof(address)) < 0) {
        std::cerr << "Bind failed" << std::endl;
        return false;
    }

    if (listen(server_fd_, 3) < 0) {
        std::cerr << "Listen failed" << std::endl;
        return false;
    }

    std::cout << "Server started, listening on " << ip_ << ":" << port_ << std::endl;

    // Start accepting clients in a new thread
    std::thread(&TCPServer::acceptClients, this).detach();

    return true;
}

void TCPServer::set_mavlink_message_callback(MavlinkMessageCallback callback) {
    message_callback_ = callback;
    std::cout<<"set_mavlink_message_callback success"<<std::endl;
}

void TCPServer::send_mavlink_message(const mavlink_message_t& message) {
    uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
    size_t len = mavlink_msg_to_send_buffer(buffer, &message);
    std::lock_guard<std::mutex> lock(clients_mutex_);
    for (int client_fd : clients_) {
        send(client_fd, buffer, len, 0);
    }
}

void TCPServer::acceptClients() {
    while (true) {
        sockaddr_in client_address;
        socklen_t client_address_len = sizeof(client_address);
        int client_fd = accept(server_fd_, (struct sockaddr*)&client_address, &client_address_len);
        if (client_fd < 0) {
            std::cerr << "Accept failed" << std::endl;
            continue;
        }

        std::cout << "Client connected" << std::endl;

        std::lock_guard<std::mutex> lock(clients_mutex_);
        clients_.push_back(client_fd);

        // Handle client messages in a new thread
        std::thread(&TCPServer::handleClient, this, client_fd).detach();
    }
}

void TCPServer::handleClient(int client_fd) {
    char buffer[1024] = {0};
    mavlink_status_t status;
    mavlink_message_t msg;
    while (true) {
        int bytes_read = recv(client_fd, buffer, sizeof(buffer), 0);
        if (bytes_read <= 0) {
            std::cerr << "Client disconnected or read error" << std::endl;
            close(client_fd);

            std::lock_guard<std::mutex> lock(clients_mutex_);
            clients_.erase(std::remove(clients_.begin(), clients_.end(), client_fd), clients_.end());

            break;
        }

        for (int i = 0; i < bytes_read; ++i) {
            if (mavlink_parse_char(MAVLINK_COMM_0, buffer[i], &msg, &status)) {
                message_callback_(msg);
            }
        }
    }
}

void TCPServer::handleMavLinkMessage(const std::string& message) {
    // Implement MAVLink message handling here
}